#include "stm32f10x.h" // Device header
#include "Delay.h"
#include "OLED.h"
#include "LED.h"
#include "Key.h"
#include "MPU6050.h"
#include "KALMAN_FILTER.h"
#include "Serial.h"
#include "Timer.h"

#include <string.h>
#include <math.h>
#include <stdio.h>
#include <stdarg.h>


#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"

// 解决多个任务同时访问相同外设资源
SemaphoreHandle_t xUartMutex = NULL;  // 串口互斥锁
SemaphoreHandle_t xOledMutex = NULL;  // OLED互斥锁


KalmanFilter kf; // 声明滤波器实例

// 定义MPU6050数据结构体
typedef struct
{
    float accel_angle;    // 原始角度
    float gyro_rate;      // 角度度
    float filtered_angle; // 过滤后的角度
} MPUData_t;

static TaskHandle_t AppTaskCreate_Handle = NULL; /* 创建任务句柄 */
static TaskHandle_t vLED_Task_Handle = NULL;     /* LED任务句柄 */
static TaskHandle_t vKEY_Task_Handle = NULL;     /* KEY任务句柄 */
static TaskHandle_t vMPU6050_Task_Handle = NULL; /* KEY任务句柄 */
static TaskHandle_t vUart_Task_Handle = NULL;    /* KEY任务句柄 */

// 队列句柄
QueueHandle_t xMPUDataQueue = NULL;

static void AppTaskCreate(void); /* 用于创建任务 */

static void vLED_Task(void *pvParameters);     /* LED_Task任务实现 */
static void vKEY_Task(void *pvParameters);     /* KEY_Task任务实现 */
static void vMPU6050_Task(void *pvParameters); /* MPU任务 */
static void vUart_Task(void *pvParameters);    /* uart任务 */

//void vMemMonitorTask(void *pvParameters); // 内存监控任务

static void BSP_Init(void); // 板载资源初始化
// 安全访问外设资源
void Safe_Serial_Printf(const char *format, ...);
void Safe_OLED_Printf(uint8_t X, uint8_t Y, uint8_t FontSize, const char *format, ...);

int main()
{
    BaseType_t xReturn = pdPASS; /* 定义一个创建信息返回值，默认为pdPASS */

    BSP_Init();
    
    
    xUartMutex = xSemaphoreCreateMutex();
    xOledMutex = xSemaphoreCreateMutex();
    configASSERT(xUartMutex != NULL);
    configASSERT(xOledMutex != NULL);

    // 初始化卡尔曼滤波器
    Kalman_Init(&kf, 0.01f, 0.002f, 0.005f);

    // 创建MPU数据队列，最多存储10组数据
    xMPUDataQueue = xQueueCreate(10, sizeof(MPUData_t));

    /* 创建AppTaskCreate任务 */
    xTaskCreate((TaskFunction_t)AppTaskCreate,          /* 任务入口函数 */
                (const char *)"AppTaskCreate",          /* 任务名字 */
                (uint16_t)512,                          /* 任务栈大小 */
                (void *)NULL,                           /* 任务入口函数参数 */
                (UBaseType_t)4,                         /* 任务的优先级 */
                (TaskHandle_t *)&AppTaskCreate_Handle); /* 任务控制块指针 */

    if (xReturn == pdPASS)
        vTaskStartScheduler(); // 启动调度器
    else
        return -1;

    // 永不执行到这里
    while (1);
        
}

static void AppTaskCreate(void)
{
    BaseType_t xReturn = pdPASS; /* 定义一个创建信息返回值，默认为pdPASS */

    taskENTER_CRITICAL(); // 进入临界区

    /* 创建LED_Task任务 */
    xReturn = xTaskCreate((TaskFunction_t)vLED_Task,          /* 任务入口函数 */
                          (const char *)"vLED_Task",          /* 任务名字 */
                          (uint16_t)512,                      /* 任务栈大小 */
                          (void *)NULL,                       /* 任务入口函数参数 */
                          (UBaseType_t)5,                     /* 任务的优先级 */
                          (TaskHandle_t *)&vLED_Task_Handle); /* 任务控制块指针 */

    /* 创建KEY_Task任务 */
    xReturn = xTaskCreate((TaskFunction_t)vKEY_Task,          /* 任务入口函数 */
                          (const char *)"vKEY_Task",          /* 任务名字 */
                          (uint16_t)512,                      /* 任务栈大小 */
                          (void *)NULL,                       /* 任务入口函数参数 */
                          (UBaseType_t)6,                     /* 任务的优先级 */
                          (TaskHandle_t *)&vKEY_Task_Handle); /* 任务控制块指针 */

    xReturn = xTaskCreate((TaskFunction_t)vUart_Task,          /* 任务入口函数 */
                          (const char *)"vUart_Task",          /* 任务名字 */
                          (uint16_t)512,                       /* 任务栈大小 */
                          (void *)NULL,                        /* 任务入口函数参数 */
                          (UBaseType_t)4,                      /* 任务的优先级 */
                          (TaskHandle_t *)&vUart_Task_Handle); /* 任务控制块指针 */

    xReturn = xTaskCreate((TaskFunction_t)vMPU6050_Task,          /* 任务入口函数 */
                          (const char *)"vMPU6050_Task",          /* 任务名字 */
                          (uint16_t)512,                          /* 任务栈大小 */
                          (void *)NULL,                           /* 任务入口函数参数 */
                          (UBaseType_t)4,                         /* 任务的优先级 */
                          (TaskHandle_t *)&vMPU6050_Task_Handle); /* 任务控制块指针 */

//    xTaskCreate(vMemMonitorTask, "MemMon", 512, NULL, 3 , NULL); // 内存监控任务

    vTaskDelay(pdMS_TO_TICKS(10)); // 等待任务创建完成

    vTaskDelete(AppTaskCreate_Handle); // 删除AppTaskCreate任务

    taskEXIT_CRITICAL(); // 退出临界区
}

// LED 任务实现
void vLED_Task(void *pvParameters)
{
    while (1)
    {
        //LED1_Turn();
        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

void vKEY_Task(void *pvParameters)
{
    while (1)
    {
        uint8_t Key = Key_GetNum();
        if (Key == 3)
        {
            LED1_Turn();
        }
        vTaskDelay(pdMS_TO_TICKS(20));
    }
}

void vMPU6050_Task(void *pvParameters)
{
    const TickType_t xFrequency = pdMS_TO_TICKS(10); // 100Hz采样率
    TickType_t xLastWakeTime = xTaskGetTickCount();

    int16_t AccX, AccY, AccZ, GyroX, GyroY, GyroZ;
    MPUData_t mpuData;
    Safe_Serial_Printf("MPU Task Running\n"); // 因为串口的互斥锁可能在规定时间内获取不到
    for (;;)
    {
        // 获取MPU6050原始数据
        MPU6050_GetData(&AccX, &AccY, &AccZ, &GyroX, &GyroY, &GyroZ);

        // 计算加速度计角度
        mpuData.accel_angle = Get_PitchAngle(AccX, AccZ);

        // 陀螺仪角速度（Y轴）
        mpuData.gyro_rate = GyroY / 16.4f;

        // 更新卡尔曼滤波器
        Kalman_Update(&kf, mpuData.gyro_rate, mpuData.accel_angle, 0.01f);
        mpuData.filtered_angle = Kalman_GetAngle(&kf);

        // OLED显示
        Safe_OLED_Printf(0, 0, OLED_8X16, "A:%.1f fA:%.1f", mpuData.accel_angle, mpuData.filtered_angle);
        

        // 发送数据到队列
        if (xMPUDataQueue != NULL)
        {
            // xQueueSend(xMPUDataQueue, &mpuData, 0);
            // 当队列满时覆盖最旧数据
            xQueueOverwrite(xMPUDataQueue, &mpuData);   
        }

        // 精确延时，保持100Hz采样率
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
    }
}

void vUart_Task(void *pvParameters)
{
    MPUData_t mpuData;
    
    const TickType_t xBlockTime = pdMS_TO_TICKS(1);
    
    // 正确的50Hz周期 (20ms)
    const TickType_t xTaskFrequency = pdMS_TO_TICKS(20);
    TickType_t xLastWakeTime = xTaskGetTickCount();
    Safe_Serial_Printf("vUart_Task_Runing\n");
    
    for (;;)
    {
        // 从队列接收数据
        if (xQueueReceive(xMPUDataQueue, &mpuData, xBlockTime) == pdPASS)
        {
            // 串口发送数据
            Safe_Serial_Printf("%.2f,%.2f,%.2f\r\n",
                          mpuData.accel_angle,
                          mpuData.filtered_angle,
                          mpuData.gyro_rate);
        }

        // 50Hz发送频率精确周期控制
        vTaskDelayUntil(&xLastWakeTime, xTaskFrequency);
    }
}

//// 内存监控任务
//void vMemMonitorTask(void *pvParameters)
//{
//    char buffer[32];
//    for (;;)
//    {
//        snprintf(buffer, sizeof(buffer), "Heap: %u/%u",
//                 xPortGetFreeHeapSize(),
//                 configTOTAL_HEAP_SIZE);

//        Safe_OLED_Printf(0, 32, OLED_8X16, buffer);
//        vTaskDelay(pdMS_TO_TICKS(3000)); // 5秒更新一次
//    }
//}

static void BSP_Init(void) // 每个资源建议到对应任务中进行初始化
{
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
    LED_Init();
    Key_Init();

    // 3. 初始化通信外设
    Serial_Init();

    // 4. 初始化显示
    OLED_Init();

    // 5. 初始化定时器
    Timer_Init();

    // 6. 初始化传感器
    MPU6050_Init();
}


// 安全的OLED显示函数(FreeRTOS互斥量)
void Safe_OLED_Printf(uint8_t X, uint8_t Y, uint8_t FontSize, const char *format, ...)
{
    char String[256];						
    va_list arg;							
    va_start(arg, format);					
    vsprintf(String, format, arg);			
    va_end(arg);	    
    if(xSemaphoreTake(xOledMutex, pdMS_TO_TICKS(20)) == pdTRUE) {
						
        OLED_ShowString(X, Y, String, FontSize);
        OLED_UpdateArea(X, Y, strlen(String) * FontSize, FontSize * 8);
        xSemaphoreGive(xOledMutex);
    }
}

// 安全的串口打印函数
void Safe_Serial_Printf(const char *format, ...)
{      
    char String[100];
    va_list arg;
    va_start(arg, format);
    vsprintf(String, format, arg);
    va_end(arg);
    if(xSemaphoreTake(xUartMutex, pdMS_TO_TICKS(10)) == pdTRUE) {

        Serial_SendString(String);
        xSemaphoreGive(xUartMutex);
    }
}



void TIM1_UP_IRQHandler(void)
{
    if (TIM_GetITStatus(TIM1, TIM_IT_Update) == SET)
    {
        Key_Tick();
        TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
    }
}


